//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <core.h>
#include <_hal.h>
#include "stubio.h"
#include "gdbstub.h"

#if (defined(_UDBAGENT) || defined(_KDBAGENT))

//==============================================================
//Description:put a char to the port for debugging
//Input: port, data
//Output:
//Return: false error; true success
//==============================================================
bool PutCharToPort(ComPort port, char data)
{
    if (BspWriteComPort(port, (uint8_t)data))
        return true;
    return false;
}

//==============================================================
//Description:Get a char from the debugging port
//Input: port
//Output:
//Return: A char if success; 0 if failed
//==============================================================
char GetCharFromPort(ComPort port)
{
    char portChar;
    if (BspReadComPort(port, (uint8_t*)&portChar))
        return (char)portChar;
    return 0;
}

//==============================================================
//Description:Get a debugging Packet from gdb or target
//Input:
//Output:pszBuf
//Return:
//==============================================================
EXTERN_C void CDECL GetDebugPacket(char * pszBuf)
{
    unsigned char nCheckSum;
    int i, nTemp;
    int nCount;
    char ch, ach[3];

    for (; ;) {
        while ((ch = (GetCharFromPort(COMPORT_DEBUG) & 0x7f)) != '$');
        nCheckSum = 0;
        nCount = 0;
        while (nCount < 1000) {
            ch = GetCharFromPort(COMPORT_DEBUG) & 0x7f;
            if (ch == '#') break;
            nCheckSum = nCheckSum + ch;
            pszBuf[nCount] = ch;
            nCount = nCount + 1;
        }

        if (nCount >= 1000) {
            PutCharToPort(COMPORT_DEBUG, '-');
            continue;
        }
        pszBuf[nCount] = 0;
        ach[0] = GetCharFromPort(COMPORT_DEBUG) & 0x7f;
        ach[1] = GetCharFromPort(COMPORT_DEBUG) & 0x7f;
        ach[2] = 0;

        if (!HexStrToByte(ach, &nTemp)) {
            PutCharToPort(COMPORT_DEBUG, '-');
            continue;
        }

        nTemp = nCheckSum;
        if (nCheckSum == (byte)nTemp) break;
        PutCharToPort(COMPORT_DEBUG, '-');
    }

    PutCharToPort(COMPORT_DEBUG, '+');

    if (pszBuf[2] == ':') {
        PutCharToPort(COMPORT_DEBUG, pszBuf[0]);
        PutCharToPort(COMPORT_DEBUG, pszBuf[1]);
        for (i = 3; i <= nCount; i++) {
            pszBuf[i - 3] = pszBuf[i];
        }
    }
    return;
}

//==============================================================
//Description:put a debugging Packet to gdb or target
//Input:pszBuf
//Output:
//Return:
//==============================================================

EXTERN_C void CDECL PutDebugPacket(const char * pszBuf)
{
    unsigned char nCheckSum;
    int nCount;
    char ch;
    do {
        PutCharToPort(COMPORT_DEBUG, '$');
        nCheckSum = 0;
        nCount = 0;

        while ((ch = pszBuf[nCount])) {
            if (!PutCharToPort(COMPORT_DEBUG, ch)) {
                return;
            }
            nCheckSum += ch;
            nCount += 1;
        }
        PutCharToPort(COMPORT_DEBUG, '#');
        PutCharToPort(COMPORT_DEBUG, c_arrHexChars[nCheckSum >> 4]);
        PutCharToPort(COMPORT_DEBUG, c_arrHexChars[nCheckSum % 16]);
    } while (GetCharFromPort(COMPORT_DEBUG) != '+');
}

EXTERN_C void CDECL AtomicGetDebugPacket(char * pszBuf)
{
    uint32_t uFlags = SaveFlagsAndCli();
    GetDebugPacket(pszBuf);
    RestoreIF(uFlags);
}

EXTERN_C void CDECL AtomicPutDebugPacket(const char * pszBuf)
{
    uint32_t uFlags = SaveFlagsAndCli();
    PutDebugPacket(pszBuf);
    RestoreIF(uFlags);
}

//since trace/debug use one COM port, we should allow usr
//enable/disable trace thru command line.
//
EXTERN_C void CDECL PutTracePacket(const void * pszBuf, size_t uLen)
{
    for (size_t i = 0; i < uLen; i++) {
        PutCharToPort(COMPORT_DEBUG, ((char *)pszBuf)[i]);
    }
}

#endif //#if (defined(_UDBAGENT) || defined(_KDBAGENT))
